#include "scene_straight.h"
/*******************************停障实现*****************************/
// 停障
StraightStopObs::StraightStopObs() {
    // 多态用法，使用父类的指针创建了一个子类的对象，子类的对象是一般道路类型
    // 就可以用父类的指针调用子类的函数
    road0 = std::make_unique<RoadNormal>();
    // 非多态，锥桶类型的指针直接调用锥桶的构造函数创建一个锥桶对象
    cone = std::make_unique<Cone>(SWIDTH / 2.0, SWIDTH / 4.0, 50.0); // 放置锥桶
    // 放置车
    car0 = std::make_unique<CarNormal>(SWIDTH / 2.0, SHEIGHT - 70.0);
    car0->speed_y = -5.0;
    car0->CoutInfo();
    ShowScene();
    system("pause");
}
// 显示
void StraightStopObs::ShowScene() {
    BeginBatchDraw();
    cleardevice();

    // 绘制道路
    road0->ShowRoad();
    // 绘制锥桶
    cone->ShowCone();
    // 绘制车辆
    car0->ShowCar(BLACK);

    EndBatchDraw();
    Delay(DELAYTIME);
}
// 处理函数
bool StraightStopObs::Planning_process() {
    // 停障是一个匀减速运动
    // 计算停止距离
    double stop_line = cone->p_center->y + cone->r + safedis;
    UniformAccByDis(car0->pmidf->y - stop_line, 0.0);
    return true;
}

/********************停靠站点实现****************************/
StraightStation::StraightStation() {
    road0 = std::make_unique<RoadNormal>();
    station = std::make_unique<Point>(SWIDTH / 2.0, SHEIGHT / 2.0);
    car0 = std::make_unique<CarNormal> (SWIDTH / 2.0, SHEIGHT - 70.0);
    car0->speed_y = -5.0;

    car0->CoutInfo();
    ShowScene();
    system("pause");
}

void StraightStation::ShowScene() {
    BeginBatchDraw();
    cleardevice();

    road0->ShowRoad();
    station->ShowPoint();
    car0->ShowCar(BLACK);

    EndBatchDraw();
    Delay(DELAYTIME);
}

bool StraightStation::Planning_process() {
    // 减速进站
    UniformAccByDis(car0->pmid->y - station->y, 0.0);
    // 等待时间
    Delay(stop_time * 1000);
    // 加速出站
    UniformAccByTime(2.0, speed_limit);
    return true;
}

/*************************跟车场景实现******************/
StraightFollow::StraightFollow() {
    road0 = std::make_unique<RoadNormal>();
    car0 = std::make_unique<CarNormal>(SWIDTH / 2.0, SHEIGHT - 70.0);
    carObs = std::make_unique<CarNormal>(SWIDTH / 2.0, SHEIGHT / 2.0, 0.0, 100.0, 50.0);
    car0->speed_y = -5.0;
    carObs->speed_y = -2.0;

    car0->CoutInfo();
    carObs->CoutInfo();
    ShowScene();
    system("pause");
}

void StraightFollow::ShowScene() {
    BeginBatchDraw();
    cleardevice();

    road0->ShowRoad();
    car0->ShowCar(BLACK);
    carObs->ShowCar(RED);

    EndBatchDraw();
    Delay(DELAYTIME);

}

bool StraightFollow::Planning_process() {
    // 初始间距
    double dis = car0->pmidf->y - carObs->pmidr->y;
    // 初始间距 - 目标间距
    double delta_dis = dis - safeDis;
    // 速度差
    double delta_speed_y = car0->speed_y - carObs->speed_y;
    // 只考虑主车车速更快需要减速的情况
    if (dis <= 0.0 || delta_speed_y > 0.0 || delta_dis <= 0.0) {
        return false;
    }
    // 计算主车加速度
    car0->a_y = std::pow(delta_speed_y, 2) / (2 * delta_dis);
    // 主车再窗口内就循环执行程序
    while (car0->pmidr->y > 0.0) {
        // 移动主车和障碍车
        car0->MoveStraightStep();
        carObs->MoveStraightStep();
        if (std::fabs(car0->speed_y - carObs->speed_y) > std::fabs(car0->a_y)) {
            car0->speed_y += car0->a_y;
        } else {
            car0->speed_y = carObs->speed_y;
            car0->a_y = 0.0;
        }
        ShowScene();

    }
    car0->CoutInfo();
    return true;
}

/***************************通过斑马线场景实现*******************/
StraightCrosswalk::StraightCrosswalk() {
    road0 = std::make_unique<RoadCrosswalk>();
    car0 = std::make_unique<CarNormal>(SWIDTH / 2.0, SHEIGHT / 2.0 + 300.0);
    car0->speed_y = -4.0;
    for (int i = 0; i < people_num; ++i) {
        // 使用auto自动推导类型，并添加对象到people向量
        auto ps = std::make_unique<Person>(road0->right_boundary + 20 * (i * 3 + 1), road0->GetMidLine());
        ps->speed = 2.0;
        // unique_ptr不共享它的指针。它无法复制到其他 unique_ptr，无法通过值传递到函数，也无法用于需要副本的任何标准模板库 (STL) 算法。
        // 只能移动unique_ptr
        // ps是一个独有指针对象，如果不使用移动语义move，相当于又复制了一份指针，这样两个指针指向同一个内存，对于unique_ptr
        // 指针来讲是不允许的，所以只能移动过去
        people.push_back(std::move(ps)); // 将对象移动到向量中
    }
    car0->CoutInfo();
    ShowScene();
    system("pause");
}
void StraightCrosswalk::ShowScene() {
    BeginBatchDraw();
    cleardevice();
    //绘制静态道路、车辆
    road0->ShowRoad();
    car0->ShowCar(BLACK);
    //移动主车
    car0->MoveStraightStep();
    car0->speed_y += car0->a_y;
    // 移动行人
    for (auto& i : people) {
        i->ShowPerson();
        i->PersonMove();
    }
    EndBatchDraw();
    Delay(DELAYTIME);
}

bool StraightCrosswalk::PersonInCross() {
    // 检查每个人是否在斑马线上
    for (auto& i : people) {
        if (i->p_center->x - i->r <= road0->right_boundary &&
            i->p_center->x + i->r >= road0->left_boundary) {
            return true;
        }
    }
    return false;
}

bool StraightCrosswalk::Planning_process() {
    
    double dis = car0->pmidf->y - road0->GetDownLine();
    car0->a_y = std::pow(car0->speed_y, 2) / 2.0 / dis;
    // 进入斑马线前，减速
    while (dis > 0.0) {
        dis = car0->pmidf->y - road0->GetDownLine();
        // 斑马线没人
        if (!PersonInCross()) {
            if (car0->speed_y >= speedlimit_cross) {
                car0->a_y = 0.0;
            } 
        } else {
            // 靠近斑马线前让车辆完全停下
            // std::cout << "0000000000000000000" << std::endl;
            if (dis <= 10.0) {
                car0->speed_y = 0.0;
                car0->a_y = 0.0;
                break;
            }
        }
        std::cout << "dis: " << dis << ", car speed_y: " << car0->speed_y << ", a_y: " << car0->a_y << std::endl;
        ShowScene();
    }
    // 限速通过斑马线
    while (car0->pmidr->y > road0->GetUpLine()) {
        // 如果斑马线没人
        if (!PersonInCross()) {
            // 没有达到斑马线限速
            if (car0->speed_y > speedlimit_cross) {
                car0->a_y = -0.5;
            } else {
                // 达到斑马线限速
                car0->a_y = 0.0;
            }
        }
        std::cout << "car speed_y: " << car0->speed_y << ", a_y: " << car0->a_y << std::endl;
        ShowScene();
    }
    // 出斑马线加速
    while (car0->pmidr->y > 1e-6) {
        if (car0->speed_y > speed_limit) {
            car0->a_y = -0.1;
        } else {
            car0->a_y = 0.0;
        }
        std::cout << "car0 speed_y: " << car0->speed_y << ", a_y: " << car0->a_y << std::endl;
        ShowScene();
    }

    return true;
}